#include "../include/tool.hpp"

float Tool::LookUpTable(float find_value, TableBP table_bp){
    float result = 0;

    //从断点的第一个开始循环
    for(auto it = table_bp.BP.begin(); it != table_bp.BP.end(); it++){
        //如果查找值比最小值还小，结果等于table的第一个值
        if(find_value <= table_bp.BP.front()){
            return table_bp.Table.front();
        }
        //如果查找值比最大值还大，结果等于table的最后一个值
        if(find_value >= table_bp.BP.back()){
            return table_bp.Table.back();
        }
        //如果查找值比当前值大,且比下一个值小,求解
        if(find_value >= *it && find_value < *(it + 1)){
            int dis = it - table_bp.BP.begin();
            float ration = (find_value - *it)/DivideProtect(*(it+1) - *it);
            result = table_bp.Table[dis] + (table_bp.Table[dis + 1] - table_bp.Table[dis])*ration;
            return result;
        }
    }
    return result;
}

float Tool::DivideProtect(float value){
    if(value == 0){
        return std::numeric_limits<float>::min();
    }
    return value;
}

float Tool::Satuation(float value, float min, float max){
    if(value < min){
        return min;
    }
    else if(value > max){
        return max;
    }
    else{
        return value;
    }
}

float Tool::Integrate(int id, float value, float min, float max, float hz, bool reset_flag){
    //如果重置flag置1，重置积分为0
    if(reset_flag == true){
        integrate_value[id] = 0.f;
    }
    //积分累加
    else{
        integrate_value[id] = integrate_value[id] + value/DivideProtect(hz);
        integrate_value[id] = Satuation(integrate_value[id], min, max);
    }
    return integrate_value[id];
}

float Tool::Derivative(int id, float value, float min, float max, float hz, bool reset_flag){
    float derivative_value = 0.f;
    long unsigned int count500ms = (long unsigned int)(hz/2);
    //如果重置flag置1，清除队列，结果置0
    if(reset_flag == true){
        while(!derivative_input[id].empty()){
            derivative_input[id].pop();
        }
        derivative_value = 0.f;
    }
    else{
        //如果时间达到500ms，按500ms进行微分
        if(derivative_input[id].size() >= count500ms){
            derivative_input[id].pop();
            derivative_input[id].push(value);
            derivative_value = (derivative_input[id].back() - derivative_input[id].front())/0.5f;
        }
        //如果时间未达到500ms，按积累时间进行微分
        else{
            derivative_input[id].push(value);
            float time = derivative_input.size()/hz;
            derivative_value = (derivative_input[id].back() - derivative_input[id].front())/DivideProtect(time);
        }
        derivative_value = Satuation(derivative_value, min, max);
    }
    return derivative_value;
}

float Tool::Delay(int id, float value, int period){
    float delay_value = 0.f;
    //如果数据累积周期没到延时周期，返回第一个值
    if(delay_input[id].size() < (long unsigned int)period){
        delay_input[id].push(value);
        delay_value = delay_input[id].front();
    }
    //如果数据累积周期到了延时周期，返回第一个值,然后pop,push一次
    else{
        delay_value = delay_input[id].front();
        delay_input[id].pop();
        delay_input[id].push(value);
    }
    return delay_value;
}

float Tool::ButterworthFilter(int id, float value, float wc, float hz){
    float t = 2*M_PI/DivideProtect(hz);
    //一阶巴特沃斯滤波器参考方程   Y(k) = (Y(k-1) + wc*T*X(k))/(1 + wc*T)
    //Y(k)：滤波输出  X(K)：滤波输入  wc：截至频率 T: 调用周期(rad)
    filter_output[id] = (filter_output[id] + wc*t*value)/DivideProtect(1.f + wc*t);
    return filter_output[id];
}

float Tool::RateLimit(int id, float value, float max_rate, float hz){
    float max_change = max_rate/DivideProtect(hz);
    float now_change = value - rate_limit_output[id];
    //判断改变值和当前周期最大允许改变值的关系
    if(now_change > max_change){
        rate_limit_output[id] += max_change;
    }
    else if(now_change < -1*max_change){
        rate_limit_output[id] += -1*max_change;
    }
    else{
        rate_limit_output[id] += now_change;
    }
    return rate_limit_output[id];
}

float Tool::StepUp(int period, float start_value, float end_value){
    if(step_up_value == -1){
        step_up_value = period;
    }
    if(step_up_value > 0){
        step_up_value--;
        return start_value;
    }
    else{
        return end_value;
    }
}